use super::sport_api_id::*;
use r2r::{self, QosProfile};
use serde_json::{Value, json};
use std::sync::mpsc::{self, Receiver};
use std::sync::{Arc, Mutex};
use std::time::{Duration, Instant};

pub struct SportClient {
    req_puber: Arc<Mutex<r2r::Publisher<r2r::unitree_api::msg::Request>>>,
    resp_rx: Receiver<r2r::unitree_api::msg::Response>,
    // keep the background spinner thread alive
    _spinner: std::thread::JoinHandle<()>,
}

impl SportClient {
    pub fn from_node(node: Arc<Mutex<r2r::Node>>) -> Result<Self, r2r::Error> {
        let req_puber = node
            .lock()
            .unwrap()
            .create_publisher::<r2r::unitree_api::msg::Request>(
                "/api/sport/request",
                QosProfile::default(),
            )?;

        let (tx, rx) = mpsc::channel::<r2r::unitree_api::msg::Response>();
        let sub = node
            .lock()
            .unwrap()
            .subscribe::<r2r::unitree_api::msg::Response>(
                "/api/sport/response",
                QosProfile::default(),
            )?;

        // 在后台线程中spin并将订阅到的消息转发到通道
        let spinner = std::thread::spawn(move || {
            use futures::{executor::LocalPool, stream::StreamExt, task::LocalSpawnExt};

            let mut pool = LocalPool::new();
            let spawner = pool.spawner();

            // 转发订阅消息到mpsc
            spawner
                .spawn_local(async move {
                    let mut sub = sub;
                    while let Some(msg) = sub.next().await {
                        let _ = tx.send(msg);
                    }
                })
                .expect("spawn_local failed");

            // let node_clone = node.clone();
            loop {
                node.lock().unwrap().spin_once(Duration::from_millis(10));
                pool.run_until_stalled();
            }
        });

        Ok(Self {
            req_puber: Arc::new(Mutex::new(req_puber)),
            resp_rx: rx,
            _spinner: spinner,
        })
    }

    // 创建一个新的Context+Node
    pub fn new(node_name: &str, namespace: &str) -> Result<Self, r2r::Error> {
        let ctx = r2r::Context::create()?;
        let node = Arc::new(Mutex::new(r2r::Node::create(ctx, node_name, namespace)?));
        Self::from_node(node.clone())
    }

    // 阻塞式等待与请求相同 api_id 的响应，返回 data 字段的 JSON
    #[allow(non_snake_case)]
    pub fn Call(&self, req: &r2r::unitree_api::msg::Request, timeout: Duration) -> Option<Value> {
        use std::sync::mpsc::RecvTimeoutError;
        let expected_api_id = req.header.identity.api_id;
        let deadline = Instant::now() + timeout;
        loop {
            let now = Instant::now();
            if now >= deadline {
                return None;
            }
            let remaining = deadline - now;
            match self.resp_rx.recv_timeout(remaining) {
                Ok(resp) => {
                    if resp.header.identity.api_id == expected_api_id {
                        return serde_json::from_str::<Value>(&resp.data).ok();
                    }
                    // 不匹配则继续等待
                }
                Err(RecvTimeoutError::Timeout) => return None,
                Err(_) => return None,
            }
        }
    }

    // --- 以下API与C++版本等价 ---
    #[allow(non_snake_case)]
    pub fn Damp(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_DAMP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn BalanceStand(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_BALANCESTAND;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn StopMove(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_STOPMOVE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn StandUp(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_STANDUP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn StandDown(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_STANDDOWN;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn RecoveryStand(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_RECOVERYSTAND;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Euler(&self, req: &mut r2r::unitree_api::msg::Request, roll: f32, pitch: f32, yaw: f32) {
        let js = json!({"x": roll, "y": pitch, "z": yaw});
        req.parameter = js.to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_EULER;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Move(&self, req: &mut r2r::unitree_api::msg::Request, vx: f32, vy: f32, vyaw: f32) {
        let js = json!({"x": vx, "y": vy, "z": vyaw});
        req.parameter = js.to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_MOVE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Sit(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_SIT;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn RiseSit(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_RISESIT;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn SpeedLevel(&self, req: &mut r2r::unitree_api::msg::Request, level: i32) {
        req.parameter = json!({"data": level}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_SPEEDLEVEL;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Hello(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_HELLO;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Stretch(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_STRETCH;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn SwitchJoystick(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_SWITCHJOYSTICK;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Content(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_CONTENT;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Pose(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_POSE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Scrape(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_SCRAPE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FrontFlip(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FRONTFLIP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FrontJump(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FRONTJUMP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FrontPounce(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FRONTPOUNCE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Dance1(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_DANCE1;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Dance2(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_DANCE2;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn Heart(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_HEART;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn StaticWalk(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_STATICWALK;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn TrotRun(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_TROTRUN;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn EconomicGait(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_ECONOMICGAIT;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn LeftFlip(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_LEFTFLIP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn BackFlip(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_BACKFLIP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn HandStand(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_HANDSTAND;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FreeWalk(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FREEWALK;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FreeBound(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FREEBOUND;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FreeJump(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FREEJUMP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn FreeAvoid(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_FREEAVOID;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn ClassicWalk(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_CLASSICWALK;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn WalkUpright(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_WALKUPRIGHT;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn CrossStep(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_CROSSSTEP;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn AutoRecoverySet(&self, req: &mut r2r::unitree_api::msg::Request, flag: bool) {
        req.parameter = json!({"data": flag}).to_string();
        req.header.identity.api_id = ROBOT_SPORT_API_ID_AUTORECOVERY_SET;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }

    #[allow(non_snake_case)]
    pub fn AutoRecoveryGet(&self, req: &mut r2r::unitree_api::msg::Request) -> Option<bool> {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_AUTORECOVERY_GET;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);

        if let Some(v) = self.Call(req, Duration::from_millis(500)) {
            if let Some(data) = v.get("data") {
                return data.as_bool();
            }
        }
        None
    }

    #[allow(non_snake_case)]
    pub fn SwitchAvoidMode(&self, req: &mut r2r::unitree_api::msg::Request) {
        req.header.identity.api_id = ROBOT_SPORT_API_ID_SWITCHAVOIDMODE;
        let puber = self.req_puber.clone();
        let _ = puber.lock().unwrap().publish(req);
    }
}
